#ifndef _COLLISION_PRIMITIVES_H__
#define _COLLISION_PRIMITIVES_H__

#include "vector.h"
#include "matrix.h"

//class ICollisionObject
//{
//public:
//	virtual ~ICollisionObject()=0;
//
//	virtual bool Collide(const ICollisionObject &) const=0;
//};
class CollisionCone;
// class CollisionPoint;
class CollisionSphere;



class CollisionRect
{
	mdsVector<3, double> m_p3Center;
	mdsVector<3, double> m_p3Size;
	mdsMatrix<3, double> m_matOrient;
	mdsMatrix<3, double> m_matInverse; // just in case some ####@@@ hands us non-orthonormal matrices

	
public:
	CollisionRect(){}

	CollisionRect(const CollisionRect & src)
	{
		m_p3Center = src.m_p3Center;
		m_p3Size = src.m_p3Size;
		m_matOrient = src.m_matOrient;
	}

	const CollisionRect & operator=(const CollisionRect & src)
	{
		m_p3Center = src.m_p3Center;
		m_p3Size = src.m_p3Size;
		m_matOrient = src.m_matOrient;
		return *this;
	}

	CollisionRect(double lfLength, double lfWidth, double lfHeight)
	{
		m_p3Size[0] = lfLength; m_p3Size[1] = lfWidth, m_p3Size[2] = lfHeight;
	}

	CollisionRect(mdsVector<3, double> p3Size)
	{
		m_p3Size = p3Size;
	}

	void TranslateBy(const mdsVector<3, double> & p3Xlate)
	{
		m_p3Center += p3Xlate;
	}

	void RotateBy(const mdsMatrix<3, double> m3Rotate)
	{
		bool bSingular;
		mdsMatrix<3, double> matRotInv = m3Rotate.getInverse(bSingular);

		if(bSingular)
		{
			return;
		}

		m_p3Center = m3Rotate.preMultVec(m_p3Center);
		m_matOrient = m_matOrient.multMat(m3Rotate);
		m_matInverse = matRotInv.multMat(m_matInverse);
	}

	bool CollidePoint(const mdsVector<3, double> & p3PointIn) const
	{
		mdsVector<3, double> p3Point = p3PointIn - m_p3Center;
		p3Point = m_matInverse.preMultVec(p3Point);
		return (fabs(p3Point[0]) < m_p3Size[0]) &&
			(fabs(p3Point[1]) < m_p3Size[1]) && 
			(fabs(p3Point[2]) < m_p3Size[2]);
	}
		
	bool CollideRay(const mdsRay<double> & rayIn, mdsVector<3, double> vecResult) const;

	mdsVector<3, double> GetCenter() const { return m_p3Center; }
	mdsMatrix<3, double> GetOrient() const { return m_matOrient; }
	mdsVector<3, double> GetSize() const { return m_p3Size; }
};

class CollisionCone // : public ICollisionObject
{
	mdsVector<3, double> m_p3Apex;
	mdsVector<3, double> m_p3Dir;
	double m_lfConeSlope;
public:
	CollisionCone():m_lfConeSlope(1.0) {m_p3Dir[2] = 1.0; }
	virtual ~CollisionCone() {}

	CollisionCone(mdsVector<3, double> p3Apex, 
		mdsVector<3, double> p3Dir, 
		double lfConeSlope)
	{
		m_p3Apex = p3Apex; m_p3Dir = p3Dir; m_lfConeSlope = lfConeSlope;
	}

	CollisionCone(const CollisionCone & src)
	{
		m_p3Apex = src.m_p3Apex;
		m_p3Dir = src.m_p3Dir;
		m_lfConeSlope = src.m_lfConeSlope;
	}

	const CollisionCone & operator=(const CollisionCone & src)
	{
		m_p3Apex = src.m_p3Apex;
		m_p3Dir = src.m_p3Dir;
		m_lfConeSlope = src.m_lfConeSlope;
		return *this;
	}

	bool CollidePoint(const mdsVector<3, double> & p3Point) const
	{
		mdsVector<3, double> p3RelPoint(p3Point);
		p3RelPoint -= m_p3Apex;
		double lfDistOnRaySqrd = p3RelPoint.Dot(m_p3Dir);
		if(lfDistOnRaySqrd < 0.0)
		{
			return false;
		}
		lfDistOnRaySqrd *= lfDistOnRaySqrd;
		double lfLenSqrd = p3RelPoint.Dot(p3RelPoint);
		double lfPointSlopeSqrd = (lfLenSqrd-lfDistOnRaySqrd)/lfDistOnRaySqrd;
		return (lfPointSlopeSqrd <= (m_lfConeSlope*m_lfConeSlope));
	}

	mdsVector<3, double> GetApex() const { return m_p3Apex; }
	mdsVector<3, double> GetDir() const { return m_p3Dir; }

	void TranslateBy(const mdsVector<3, double> & p3Xlate)
	{
		m_p3Apex += p3Xlate;
	}

	void RotateBy(const mdsMatrix<3, double> m3Rotate)
	{
		m_p3Apex = m3Rotate.preMultVec(m_p3Apex);
		m_p3Dir = m3Rotate.preMultVec(m_p3Dir);
	}
};



#endif
